#include "ToolFunction.h"

//extern UART_HandleTypeDef huart1;

/**
 * @brief  对点坐标结构体里的X_Val和Y_Val五个数据去头掐尾取平均
 * @param  点坐标结构体
 * @return 
 */
void Tool_Filter_3_5(Point_InitDefStruct *PointProto)
{
	float temp;
	for(int i=0;i<4;i++)
	{
		if(PointProto->X_Val[i]>PointProto->X_Val[i+1])
		{
			temp = PointProto->X_Val[i];
			PointProto->X_Val[i] = PointProto->X_Val[i+1];
			PointProto->X_Val[i+1] = temp;
		}
	}
			
	for(int i=4;i>0;i--)
	{
		if(PointProto->X_Val[i]<PointProto->X_Val[i-1])
		{
			temp = PointProto->X_Val[i];
			PointProto->X_Val[i] = PointProto->X_Val[i-1];
			PointProto->X_Val[i-1] = temp;
		}
	}
	
	for(int i=0;i<4;i++)
	{
		if(PointProto->Y_Val[i]>PointProto->Y_Val[i+1])
		{
			temp = PointProto->Y_Val[i];
			PointProto->Y_Val[i] = PointProto->Y_Val[i+1];
			PointProto->Y_Val[i+1] = temp;
		}
	}
			
	for(int i=4;i>0;i--)
	{
		if(PointProto->Y_Val[i]<PointProto->Y_Val[i-1])
		{
			temp = PointProto->Y_Val[i];
			PointProto->Y_Val[i] = PointProto->Y_Val[i-1];
			PointProto->Y_Val[i-1] = temp;
		}
	}
	
	PointProto->X_Ave = (PointProto->X_Val[1]+PointProto->X_Val[2]+PointProto->X_Val[3])/3;
	PointProto->Y_Ave = (PointProto->Y_Val[1]+PointProto->Y_Val[2]+PointProto->Y_Val[3])/3;
}


/**
 * @brief  MPU6050,180度与-180度跳变滤波
 * @param  模式
 * @param  Yaw值
 * @return Yaw值
 */
float Tool_Filter_MPU6050(FLITER_Mode mode, float Yaw_Value)
{
	switch (mode)
    {
        //全部变为负值
		case FLITER_NEGATIVE:
            if(Yaw_Value > 0)
				Yaw_Value-= 360;
			return Yaw_Value;
				break;
				
        //全部变为正值
		case FLITER_POSITIVE:
            if(Yaw_Value < 0)
				Yaw_Value+= 360;
			return Yaw_Value;
				break;
			
        default:
				break;
    }
}


/**
 * @brief  卡尔曼一阶滤波函数
 * @param  待滤波值
 * @param  卡尔曼结构体地址
 * @return kalman->X
 */
float Tool_Filter_Kalman_1(float Filter_Value, Kalman_1_DefStruct *kalman)
{
	//prediction
	kalman->X_ = kalman->X  + 0;
	kalman->P_ = kalman->P  + kalman->Q;

	//update
	kalman->K  = kalman->P_ / (kalman->P_ + kalman->R);
	kalman->X  = kalman->X_ + kalman->K*(Filter_Value - kalman->X_);
	kalman->P  = kalman->P_ - kalman->K*kalman->P_;

	return kalman->X;
}


/**
 * @brief  卡尔曼二阶滤波函数（6050专用） 
 * @param  角加速度
 * @param  角速度
 * @param  卡尔曼结构体地址
 * @return 角度
 */
float Tool_Filter_Kalman_2(float acc,float gyro,Kalman_2_DefStruct *kalman) 			
{
	float Kal_Val;

	//预测状态方程
	Kal_Val += (gyro - kalman->Q_bias) * kalman->dt;	//状态方程,角度值等于上次最优角度加角速度减零漂后积分

	//预测协方差方程
	kalman->PP[0][0] = kalman->PP[0][0] + kalman->Q_angle - (kalman->PP[0][1] + kalman->PP[1][0])*kalman->dt;
	kalman->PP[0][1] = kalman->PP[0][1] - kalman->PP[1][1]*kalman->dt;
	kalman->PP[1][0] = kalman->PP[1][0] - kalman->PP[1][1]*kalman->dt;
	kalman->PP[1][0] = kalman->PP[1][0] - kalman->PP[1][1]*kalman->dt;
	kalman->PP[1][1] = kalman->PP[1][1] + kalman->Q_gyro;

	//卡尔曼增益方程
	kalman->K_0 = kalman->PP[0][0] / (kalman->PP[0][0] + kalman->R_angle);
	kalman->K_1 = kalman->PP[1][0] / (kalman->PP[0][0] + kalman->R_angle);

	//更新最优值方程
	Kal_Val = Kal_Val + kalman->K_0 * (acc - Kal_Val);
	kalman->Q_bias = kalman->Q_bias + kalman->K_1 * (acc - Kal_Val);

	//更新协方差方程
	kalman->PP[0][0] = kalman->PP[0][0] - kalman->K_0 * kalman->PP[0][0];
	kalman->PP[0][1] = kalman->PP[0][1] - kalman->K_0 * kalman->PP[0][1];
	kalman->PP[1][0] = kalman->PP[1][0] - kalman->K_1 * kalman->PP[0][0];
	kalman->PP[1][1] = kalman->PP[1][1] - kalman->K_1 * kalman->PP[0][1];

	return Kal_Val;
}


/**
 * @brief  指数函数
 * @param  m 底数
 * @param  n 指数
 * @return 结果
 */
int Tool_POW(int m,int n)
{
	int result=1;	 
	while(n--)result*=m;
	return result;
}


/**
 * @brief  十-二转换
 * @param  n 十进制
 * @return 结果
 */
int Tool_D2B(int n) 
{
    int binaryNum = 0;
    int base = 1; // 用于二进制位的计算

    while (n > 0) {
        // 取模得到当前二进制位
        int last_digit = n % 2;
        // 使用基数将这个位转换为十进制位置
        binaryNum = binaryNum + last_digit * base;
        
        // 更新n和基数
        n = n / 2;
        base = base * 10;
    }
    
    return binaryNum;
}


///**
// * @brief 利用CPU循环实现的非精准应用的微秒延时函数 
// * @param 微秒数
// * @return 
// */
//void  Tool_Delay_us(uint32_t us)
//{
//    uint32_t delay = (HAL_RCC_GetHCLKFreq() / 8000000 * us); //使用HAL_RCC_GetHCLKFreq()函数获取主频值，经算法得到1微秒的循环次数
//    while (delay--); 										 //循环delay次，达到1微秒延时
//}


///**
// * @brief 重定向c库函数printf到DEBUG_USARTx 
// * @param 
// * @return 
// */
//int fputc(int ch, FILE *f)
//{
//  HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xffff);
//  return ch;
//}
// 

///**
// * @brief 重定向c库函数getchar,scanf到DEBUG_USARTx
// * @param 
// * @return 
// */
//int fgetc(FILE *f)
//{
//  uint8_t ch = 0;
//  HAL_UART_Receive(&huart1, &ch, 1, 0xffff);
//  return ch;
//}



